﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;

namespace RTS
{
	public class Position2D : IEquatable<Position2D>
	{
		private static readonly float errorAllowedComparation = (float)Math.Pow(10, -2);

		private float x = 0;
		public float X
		{
			get{ return x; }
			set{ x = value; }
		}

		private float y = 0;
		public float Y
		{
			get{ return y; }
			set{ y = value; }
		}

		public Position2D(float x, float y)
		{
			setValues(x, y);
		}

		public Position2D clone()
		{
			return new Position2D(this.x, this.y);
		}

		public void add( float x, float y )
		{
			addToX(x);
			addToY(y);
		}

		public void addToX(float x)
		{
			this.x += x;
		}

		public void addToY(float y)
		{
			this.y += y;
		}

		public void normalize()
		{
			double value = Math.Pow((double)x, 2) + Math.Pow((double)y, 2);
			double total = Math.Pow(value, 0.5);
			x = (float)(x / total);
			y = (float)(y / total);
		}


		/// <summary>
		///  Returns a normalized vector that has been rectified, its directions will be rounded to one of 8 posibles
		///  separated by 45 degrees.
		/// </summary>
		public void rectificate()
		{
			double direction = this.getDirection();

			if (Math.PI * 15 / 8 < direction || direction < Math.PI * 1 / 8) setValues(1, 0);
			else if (Math.PI * 1 / 8 < direction && direction < Math.PI * 3 / 8) setValues(1, 1);
			else if (Math.PI * 3 / 8 < direction && direction < Math.PI * 5 / 8) setValues(0, 1);
			else if (Math.PI * 5 / 8 < direction && direction < Math.PI * 7 / 8) setValues(-1, 1);
			else if (Math.PI * 7 / 8 < direction && direction < Math.PI * 9 / 8) setValues(-1, 0);
			else if (Math.PI * 9 / 8 < direction && direction < Math.PI * 11 / 8) setValues(-1, -1);
			else if (Math.PI * 11 / 8 < direction && direction < Math.PI * 13 / 8) setValues(0, -1);
			else if (Math.PI * 13 / 8 < direction && direction < Math.PI * 15 / 8) setValues(1, -1);

			normalize();
		}

		public void multiplyBy(float multiplier)
		{
			x = x * multiplier;
			y = y * multiplier;
		}

		public bool Equals(Position2D other)
		{
			bool xEqual = (this.X < other.X + errorAllowedComparation && this.X > other.X - errorAllowedComparation) ? true : false;
			bool yEqual = (this.Y < other.Y + errorAllowedComparation && this.Y > other.Y - errorAllowedComparation) ? true : false;

			return xEqual && yEqual;
		}

		public double getDirection()
		{
			if (this.x > 0 && this.y >= 0)
			{
				return Math.Atan(this.y / this.x);
			}
			else if (this.x < 0)
			{
				return Math.Atan(this.y / this.x) + Math.PI;
			}
			else if (this.x > 0 && this.y < 0)
			{
				return Math.Atan(this.y / this.x) + 2 * Math.PI;
			}
			else // this.x == 0
			{
				if (this.y > 0) return Math.PI / 2;
				return Math.PI * 3 / 2;
			}
			
		}

		private void setValues(float x, float y)
		{
			this.x = x;
			this.y = y;
		}

		public static float getDistanceBetween(Position2D posA, Position2D posB)
		{
			double Xdif = (double)posA.X - (double)posB.X;
			double Ydif = (double)posA.Y - (double)posB.Y;

			return (float)Math.Pow(Math.Pow(Xdif, 2) + Math.Pow(Ydif, 2), 1 / 2F);
		}

		public override string ToString()
		{
			return "X:" + x.ToString() + " Y:" + y.ToString();
		}
	}
}
